/*
 * Center then drive to the end of the hall, 
 * turn around and return to the origional position.
 *
 * Tests Player 3's functions
 */

#include <libplayerc++/playerc++.h>
#include <iostream>

#include "args.h"

#define RAYS 32

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;
    
    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    SonarProxy sp(&robot, gIndex);

    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);
    lp.Configure(0, 4.71238, 100, 1000, false, 10000);
    
    double newspeed = 0;
    double newturnrate = 0;
    double minR = 1e9;
    double minL = 1e9;
    //int lloc = 0;
    //int rloc = 0;
    //int temp;
    

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      /* Simple accessor functions
      std::cout << "Max Range of laser " << lp.GetMaxRange() << std::endl;
      std::cout << "Number of points in scan " << lp.GetCount() << std::endl;
      std::cout << "Bearing " << lp.GetBearing(135) << std::endl;
      getchar();
      */

      /* GetRange() 
      std::cout << "Testing GetRange \n"; double range;
      range = lp.GetRange(269); // far bottom left
      //range = lp.GetRange(0); // far bottom right
      //range = lp.GetRange(135); // directly in front
      std::cout << "Range: " << range << std::endl;
      range = lp[269];
      std::cout << "Using []: " << range << std::endl;
      getchar();
      */

      /* GetMinRight and GetMinLeft */
      std::cout << "Testing GetMinRight & GetMinLeft \n";
      double minR = lp.GetMinRight();
      double minL = lp.GetMinLeft();
      //double sonar = sp[0]; //sp.GetScan(4); 
      std::cout << "minR: " << minR << std::endl
                << "minL: " << minL << std::endl
	//<< "SONAR: " << sonar << std::endl
                << std::endl;
      /*
      if(minR <= 0.5){
	std::cout << "Oh-shit!\n";
	newspeed = 0.0;
	newturnrate = 0.375;
      }
      else if(minL <= 0.5){
	std::cout << "Oh-shit!\n";
	newspeed = 0.0;
	newturnrate = 0.375;
      }
      else if(minR <= 1.0){
	std::cout << "Uh-oh!\n";
	newspeed = 0.2;
	newturnrate = 0.375;
      }
     
      else if(minL <= 1.0){
	std::cout << "Uh-oh!\n";
	newspeed = 0.2;
	newturnrate = -0.375;
      }
      else{
	newspeed = 0.5;
	newturnrate = 0;
      }
      
      pp.SetSpeed(newspeed, newturnrate);
      */
      getchar();
      
    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
